www.gusucode.com > Modeling Pneumatic Robot Actuators 工具箱matlab源码程序 > Modeling Pneumatic Robot Actuators/MSRA_PneumaticRobotSeries/Libraries/MultipartsLib/Extrusion_Scripts/Extr_Data_Round_Corners.m

    function [ verticesOut ] = Extr_Data_Round_Corners( vertices, vIdxs , R, nP)
%EXTR_DATA_ROUND_CORNERS Rounds the corner between the straight line sections
% vertices(iIdx-1) -> vertices(iIdx) and vertices(iIdx) -> vertices(iIdx+1).
% iIdx is an element in the indices vector vIdxs. For each corner that is 
% rounded, nP additional vertices are created. The original vertex 
% corresponding to the corner to be rounded is replaced with these nP 
% additional vertices. This increases the number of vertices by nP-1. If 
% the first vertex is specified as one of the corners to be rounded, then 
% it is assumed that the first and last vertices are connected to form a 
% polygon. The corner formed by the first and last straight line segments 
% is rounded. The same is true if the last vertex is specified as a corner 
% to be rounded. If the indices vector has repeated indices, then the
% repetitions are removed without any warning being issued.
%
% vIdxs - 1D array of vertex indices where the corner is to be rounded.
% R     - Radius of the rounded corner. This can be a scalar or a vector
%         If it is a scalar, all corners are rounded with the same radius.
%         If it is a vector it has to be the same size as vIdxs.
% nP    - Number of points on the rounded corner

% Copyright 2012-2018 The MathWorks, Inc.

vsz = size(vertices);
assert((vsz(2)==2) && (vsz(1)>=3), ...
       'The first input has to be a Nx2 matrix of 2D vertices with N >=3');
assert(isvector(vIdxs) && isnumeric(vIdxs), ...
       'The second input has to be a 1D array of indices');
assert(all(isnumeric(R)) && all(R>0), ...
            'The third input (radius) has to be a positive number');
if ~isscalar(R)
    assert(length(R)==length(vIdxs), ...
          ['The third input has to be a scalar radius value or a vector' ...
           'of radii of the same size as the number of corners to be ' ...
           'rounded']);
end
assert(isnumeric(nP) && (nP>1), ...
       ['The fourth input (number of points on arc) has to be an ' ...
        'integer greater than 1']);    
   

% Sort indices and remove repeated indices silently.
[vIdxs, iV] = unique(vIdxs);
if isscalar(R)
    R = R*ones(length(vIdxs),1);
else
   R = R(iV);
end
verticesOut = vertices;

nAdded = 0;
for idx=1:length(vIdxs)
    iIdx = vIdxs(idx);
    
    % Check inputs
    assert((iIdx>0) && (iIdx<=vsz(1)), ...
            'The index %d is not in the valid range [1,%d]',iIdx,vsz(1));
    
    vtx1 = []; vtx3 = [];
    vtx2 = vertices(iIdx,:);
    if iIdx==1
        % Need to round the edge between segments
        % V1->V2 and Vn->V1 where Vn is the last vertex
        vtx1 = vertices(end,:);
    else
        vtx1 = vertices(iIdx-1,:);
    end
    if iIdx==vsz(1)
        % Need to round the edge between segments
        % V1->V2 and Vn->V1 where Vn is the last vertex
        vtx3 = vertices(1,:);
    else
        vtx3 = vertices(iIdx+1,:);
    end

    % The corner is between the straight line segments 
    % (vtx1->vtx2) and (vtx2->vtx3)
    
    vec21 = vtx2-vtx1; n21 = norm(vec21);
    assert(n21>sqrt(eps), ...
      'Vertices are %d and %d same. Cannot round the corner.',iIdx-1,iIdx);
    vec21 = vec21/n21;
    
    vec32 = vtx3-vtx2; n32 = norm(vec32);
    assert(n32>sqrt(eps), ...
      'Vertices %d and %d are same. Cannot round the corner.',iIdx,iIdx+1);
    vec32 = vec32/n32;
    
    phi = acos(dot(vec21,vec32));
    assert(abs(phi)>sqrt(eps), ...
           ['The straight line segments are parallel. ' ...
            'Cannot round corner at vertex %d'],iIdx);
    assert(abs(phi-pi)>sqrt(eps), ...
           ['The straight line segments are anti-parallel. ' ...
            'Cannot round corner at vertex %d'],iIdx);       
    vecd = (vec32-vec21); vecd = vecd/norm(vecd);
    scale = R(idx)/cos(phi/2);

    % Center of the rounded corner
    center = vertices(iIdx,:) + vecd*scale;

    a11 = 0; a12 = 0; a21 = 0; a22 = 0; c11 = 0; c21 = 0;
    % Equation of first segment
    a11 = -vec21(2); a12 = vec21(1); 
    c11 = vtx1(2)*vec21(1) - vtx1(1)*vec21(2);
    
    % Equation of perpendicular through the center of the arc.
    m2 = -vec21(1)/vec21(2);
    if isinf(m2)
        c21 = center(1);
        a21 = 1; a22 = 0;
    else
        c21 = center(2) - m2*center(1);
        a21 = -m2; a22 = 1;
    end

    % Intersection of first segment and its perpendicular through the
    % center of the arc.
    A = [a11 a12; a21 a22]; C = [c11; c21];
    i1 = (A\C)';

    % Equation of second segment
    a11 = -vec32(2); a12 = vec32(1); 
    c11 = vtx3(2)*vec32(1) - vtx3(1)*vec32(2);
    
    % Equation of perpendicular through the center of the arc
    m2 = -vec32(1)/vec32(2);
    if isinf(m2)
        c21 = center(1);
        a21 = 1; a22 = 0;
    else
        c21 = center(2) - m2*center(1);
        a21 = -m2; a22 = 1;
    end

    % Intersection of second segment and its perpendicular through the
    % center of the arc.
    A = [a11 a12; a21 a22]; C = [c11; c21];
    i2 = (A\C)';

    n1 = i1-center; n1 = n1/norm(n1);
    n2 = i2-center; n2 = n2/norm(n2);

    ang1 = atan2(n1(2),n1(1));
    ang2 = atan2(n2(2),n2(1));
    if abs(ang1-ang2) > pi
        if ang2 < 0
            ang2 = 2*pi+ang2;
        else
            ang2 = -2*pi+ang2;
        end
    end

    theta = linspace(ang1,ang2,nP)';

    fverts = [R(idx)*cos(theta)+center(1) R(idx)*sin(theta)+center(2)];
    
    if iIdx == 1
        verticesOut = [fverts;
                       verticesOut(2:end,:)];
    elseif iIdx == vsz(1)
        verticesOut = [verticesOut(1:iIdx-1+nAdded,:);
                       fverts];
    else
        verticesOut = [verticesOut(1:iIdx-1+nAdded,:);
                       fverts;
                       verticesOut(iIdx+1+nAdded:end,:)];
    end
    nAdded = nAdded + nP -1;
end